#include "Matrix4.h"
#include <vector>


Matrix4::Matrix4()
{
/*[0]*/     m_data[4]  = m_data[8]  = m_data[12]= 
m_data[1]  =  /*[5]*/     m_data[9]  = m_data[13]= 
m_data[2]  = m_data[6]  = /*[10] */    m_data[14]= 
m_data[3]  = m_data[7]  = m_data[11]    /*[15]*/ = 0;

m_data[0] = m_data[5] = m_data[10] = m_data[15] = 1;
}

Matrix4::Matrix4(const float* d)
{
    for( int i = 0; i< 16; i++)
    {
        m_data[i] = *d;
        d++;
    }
}
void Matrix4::LoadIdentity()
{
    /*[0]*/     m_data[4]  = m_data[8]  = m_data[12]= 
    m_data[1]  =  /*[5]*/     m_data[9]  = m_data[13]= 
    m_data[2]  = m_data[6]  = /*[10] */    m_data[14]= 
    m_data[3]  = m_data[7]  = m_data[11]    /*[15]*/ = 0;

    m_data[0] = m_data[5] = m_data[10] = m_data[15] = 1;
}
Matrix4& Matrix4::SetDiagonals(float a, float b, float c)
{
    m_data[0] *= a;
    m_data[5] *= b;
    m_data[10] *= c;
    return *this;
}    
Matrix4 Matrix4::operator*(const Matrix4& o) const
{
    Matrix4 result;
    /* First row*/ 
    result.m_data[0]  = (m_data[0]*o.m_data[0])  + (m_data[4]*o.m_data[1])  + (m_data[8]*o.m_data[2])   + (m_data[12] * o.m_data[3]);
    result.m_data[4]  = (m_data[0]*o.m_data[4])  + (m_data[4]*o.m_data[5])  + (m_data[8]*o.m_data[6])   + (m_data[12] * o.m_data[7]);
    result.m_data[8]  = (m_data[0]*o.m_data[8])  + (m_data[4]*o.m_data[9])  + (m_data[8]*o.m_data[10])  + (m_data[12] * o.m_data[11]);
    result.m_data[12] = (m_data[0]*o.m_data[12]) + (m_data[4]*o.m_data[13]) + (m_data[8]*o.m_data[14])  + (m_data[12] * o.m_data[15]);
    /* Second row */
    result.m_data[1]  = (m_data[1]*o.m_data[0])  + (m_data[5]*o.m_data[1])  + (m_data[9]*o.m_data[2])   + (m_data[13] * o.m_data[3]);
    result.m_data[5]  = (m_data[1]*o.m_data[4])  + (m_data[5]*o.m_data[5])  + (m_data[9]*o.m_data[6])   + (m_data[13] * o.m_data[7]);
    result.m_data[9]  = (m_data[1]*o.m_data[8])  + (m_data[5]*o.m_data[9])  + (m_data[9]*o.m_data[10])  + (m_data[13] * o.m_data[11]);
    result.m_data[13] = (m_data[1]*o.m_data[12]) + (m_data[5]*o.m_data[13]) + (m_data[9]*o.m_data[14])  + (m_data[13] * o.m_data[15]);
    /* Third row */
    result.m_data[2]  = (m_data[2]*o.m_data[0])  + (m_data[6]*o.m_data[1])  + (m_data[10]*o.m_data[2])   + (m_data[14] * o.m_data[3]);
    result.m_data[6]  = (m_data[2]*o.m_data[4])  + (m_data[6]*o.m_data[5])  + (m_data[10]*o.m_data[6])   + (m_data[14] * o.m_data[7]);
    result.m_data[10] = (m_data[2]*o.m_data[8])  + (m_data[6]*o.m_data[9])  + (m_data[10]*o.m_data[10])  + (m_data[14] * o.m_data[11]);
    result.m_data[14] = (m_data[2]*o.m_data[12]) + (m_data[6]*o.m_data[13]) + (m_data[10]*o.m_data[14])  + (m_data[14] * o.m_data[15]);
    /* Fourth row */
    result.m_data[3]  = (m_data[3]*o.m_data[0])  + (m_data[7]*o.m_data[1])  + (m_data[11]*o.m_data[2])   + (m_data[15] * o.m_data[3]);
    result.m_data[7]  = (m_data[3]*o.m_data[4])  + (m_data[7]*o.m_data[5])  + (m_data[11]*o.m_data[6])   + (m_data[15] * o.m_data[7]);
    result.m_data[11] = (m_data[3]*o.m_data[8])  + (m_data[7]*o.m_data[9])  + (m_data[11]*o.m_data[10])  + (m_data[15] * o.m_data[11]);
    result.m_data[15] = (m_data[3]*o.m_data[12]) + (m_data[7]*o.m_data[13]) + (m_data[11]*o.m_data[14])  + (m_data[15] * o.m_data[15]);
    return result;
}    


Vec3f Matrix4::operator*(const Vec3f& v) const
{
    return Vec3f(v.GetX() * m_data[0] + 
                 v.GetY() * m_data[4] +
                 v.GetZ() * m_data[8] + m_data[12],
                 
                 v.GetX() * m_data[1] + 
                 v.GetY() * m_data[5] + 
                 v.GetZ() * m_data[9] + m_data[13],
                 
                 v.GetX() * m_data[2] + 
                 v.GetY() * m_data[6] + 
                 v.GetZ() * m_data[10] + m_data[14]
    );
}
Vec4f Matrix4::operator*(const Vec4f& v) const
{
    return Vec4f( v.GetX() * m_data[0] + 
                  v.GetY() * m_data[4] + 
                  v.GetZ() * m_data[8] + 
                  v.GetW() * m_data[12], 
                  v.GetX() * m_data[1] + 
                  v.GetY() * m_data[5] + 
                  v.GetZ() * m_data[9] + 
                  v.GetW() * m_data[13], 
                  v.GetX() * m_data[2] + 
                  v.GetY() * m_data[6] + 
                  v.GetZ() * m_data[10] + 
                  v.GetW() * m_data[14], 
                  v.GetX() * m_data[3] + 
                  v.GetY() * m_data[7] + 
                  v.GetZ() * m_data[11] + 
                  v.GetW() * m_data[15]);
}
Vec3f Matrix4::Transform(const Vec3f& v) const
{
    return (*this) * v;
}
Vec3f Matrix4::TransformInverse(const Vec3f& vec) const
{
    Vec3f temp = vec;
    temp.SetX( temp.GetX() - m_data[12]);
    temp.SetY( temp.GetY() - m_data[13]);
    temp.SetZ( temp.GetZ() - m_data[14]);

    return Vec3f(
        temp.GetX() * m_data[0] + 
        temp.GetY() * m_data[1] + 
        temp.GetZ() * m_data[2],

        temp.GetX() * m_data[4] +
        temp.GetY() * m_data[5] +
        temp.GetZ() * m_data[6],

        temp.GetX() * m_data[8] +
        temp.GetY() * m_data[9] +
        temp.GetZ() * m_data[10]);
        
}
float Matrix4::GetDeterminant() const
{
    return (m_data[2] * m_data[5] * m_data[8])  + 
           (m_data[1] * m_data[6] * m_data[8])  + 
           (m_data[2] * m_data[4] * m_data[9])  -
           (m_data[0] * m_data[6] * m_data[9])  -
           (m_data[1] * m_data[4] * m_data[10]) +
           (m_data[0] * m_data[5] * m_data[10]);

}
void Matrix4::SetInverse(const Matrix4& m)
{
    // Make sure the determinant is non-zero.
    float det = m.GetDeterminant();
    if (det == 0) return;
    det = (1.0f)/det;

    m_data[0] = (-m.m_data[6]*m.m_data[9]+m.m_data[5]*m.m_data[10])*det;
    m_data[1] = (m.m_data[2]*m.m_data[9]-m.m_data[1]*m.m_data[10])*det;
    m_data[2] = (-m.m_data[2]*m.m_data[5]+m.m_data[1]*m.m_data[6]* m.m_data[15])*det;

    m_data[4] = (m.m_data[6]*m.m_data[8]-m.m_data[4]*m.m_data[10])*det;
    m_data[5] = (-m.m_data[2]*m.m_data[8]+m.m_data[0]*m.m_data[10])*det;
    m_data[6] = (m.m_data[2]*m.m_data[4]-m.m_data[0]*m.m_data[6]* m.m_data[15])*det;

    m_data[8] = (-m.m_data[5]*m.m_data[8]+m.m_data[4]*m.m_data[9]* m.m_data[15])*det;
    m_data[9] = (+m.m_data[2]*m.m_data[8]-m.m_data[0]*m.m_data[9]* m.m_data[15])*det;
    m_data[10] = (-m.m_data[1]*m.m_data[4]+m.m_data[0]*m.m_data[5]* m.m_data[15])*det;

    m_data[12] = (  m.m_data[6]*m.m_data[9]*m.m_data[12]
                   -m.m_data[5]*m.m_data[10]*m.m_data[12]
                   -m.m_data[6]*m.m_data[8]*m.m_data[13]
                   +m.m_data[4]*m.m_data[10]*m.m_data[13]
                   +m.m_data[5]*m.m_data[8]*m.m_data[14]
                   -m.m_data[4]*m.m_data[9]*m.m_data[14])*det;
    m_data[13] = ( -m.m_data[2]*m.m_data[9]*m.m_data[12]
                   +m.m_data[1]*m.m_data[10]*m.m_data[12]
                   +m.m_data[2]*m.m_data[8]*m.m_data[13]
                   -m.m_data[0]*m.m_data[10]*m.m_data[13]
                   -m.m_data[1]*m.m_data[8]*m.m_data[14]
                   +m.m_data[0]*m.m_data[9]*m.m_data[14])*det;
    m_data[14] =(   m.m_data[2]*m.m_data[5]*m.m_data[12]
                   -m.m_data[1]*m.m_data[6]*m.m_data[12]
                   -m.m_data[2]*m.m_data[4]*m.m_data[13]
                   +m.m_data[0]*m.m_data[6]*m.m_data[13]
                   +m.m_data[1]*m.m_data[4]*m.m_data[14]
                   -m.m_data[0]*m.m_data[5]*m.m_data[14])*det;
}
Matrix4 Matrix4::Inverse() const
{
    Matrix4 result;
    result.SetInverse(*this);
    return result;
}
Matrix4& Matrix4::Invert()
{
    SetInverse(*this);
    return *this;
}
Vec3f Matrix4::TransformDirection(const Vec3f& v) const
{
    return Vec3f(
        v.GetX() * m_data[0] + 
        v.GetY() * m_data[4] + 
        v.GetZ() * m_data[8],

        v.GetX() * m_data[1] + 
        v.GetY() * m_data[5] + 
        v.GetZ() * m_data[9],

        v.GetX() * m_data[2] + 
        v.GetY() * m_data[6] + 
        v.GetZ() * m_data[10]);
}
Vec3f Matrix4::TransformInverseDirection(const Vec3f& vec) const
{
    return Vec3f(
                vec.GetX() * m_data[0] + 
                vec.GetY() * m_data[1] + 
                vec.GetZ() * m_data[2],

                vec.GetX() * m_data[4] + 
                vec.GetY() * m_data[5] + 
                vec.GetZ() * m_data[6],

                vec.GetX() * m_data[8] + 
                vec.GetY() * m_data[9] + 
                vec.GetZ() * m_data[10]
            );
}

Vec3f Matrix4::GetAxisVecor(int index) const
{
    if(index == 1)
        return Vec3f(m_data[4], m_data[5], m_data[6]);
    else if(index == 2)
        return Vec3f(m_data[8], m_data[9], m_data[10]);
    else if(index == 3)
        return Vec3f(m_data[12], m_data[13], m_data[14]);
    //index == 0
    return Vec3f(m_data[0], m_data[1], m_data[2]);
}
Vec3f Matrix4::GetRowVector(int index) const
{
    if(index == 1)
        return Vec3f(m_data[1], m_data[5], m_data[9]);
    else if(index == 2)
        return Vec3f(m_data[2], m_data[6], m_data[10]);
    else if(index == 3)
        return Vec3f(m_data[3], m_data[7], m_data[11]);
    //index == 0
    return Vec3f(m_data[0], m_data[4], m_data[8]);
}
Matrix4& Matrix4::SetOrientationAndPosition(const Quaternion& q, const Vec3f& pos)
{            
    SetOrientation(q);
    SetTranslate(pos);
    return *this;
}
void Matrix4::FillOpenGlArray(float* arr) const
{
    for( int i = 0; i < 16; i++)
    {
        *arr = m_data[i];
        arr++;
    }
}

Matrix4& Matrix4::SetTranslate(const Vec3f& pos)
{
    m_data[12] = pos.GetX();
    m_data[13] = pos.GetY();
    m_data[14] = pos.GetZ();
    
    return *this;
}
Matrix4& Matrix4::SetTranslate(const float x, const float y, const float z)
{
    m_data[12] = x;
    m_data[13] = y;
    m_data[14] = z;

    return *this;
}
Matrix4& Matrix4::SetOrientation(const Quaternion& q)
{
    float xx = q.m_i * q.m_i;
    float xy = q.m_i * q.m_j;
    float xz = q.m_i * q.m_k;
    float xw = q.m_i * q.m_r;

    float yy = q.m_j * q.m_j;
    float yz = q.m_j * q.m_k;
    float yw = q.m_j * q.m_r;

    float zz = q.m_k * q.m_k;
    float zw = q.m_k * q.m_r;

    m_data[0] = 1 - 2 * (yy + zz);
    m_data[4] =     2 * (xy + zw);
    m_data[8] =     2 * (xz - yw);

    m_data[1] =     2 * (xy - zw);
    m_data[5] = 1 - 2 * (xx + zz);
    m_data[9] =     2 * (yz + xw);

    m_data[2] =     2 * (xz + yw);
    m_data[6] =     2 * (yz - xw);
    m_data[10] =1 - 2 * (xx + yy);
    return *this;
}
Matrix4& Matrix4::SetRotationY(float angle)
{
    Quaternion q(Vec3f(0,1,0), angle);
    SetOrientation(q);
    return *this;
}

Matrix4& Matrix4::SetRotationX(float angle)
{
    Quaternion q(Vec3f(1,0,0), angle);
    SetOrientation(q);
    return *this;
}

Matrix4& Matrix4::SetRotationZ(float angle)
{
    Quaternion q(Vec3f(0,0,1), angle);
    SetOrientation(q);
    return *this;
}

Matrix4& Matrix4::SetScale(const Vec3f& s)
{
    m_data[0] *= s.GetX();
    m_data[5] *= s.GetY();
    m_data[10] *= s.GetZ();
    return *this;
}

Matrix4& Matrix4::SetScale(const float x, const float y, const float z)
{
    m_data[0] *= x;
    m_data[5] *= y;
    m_data[10] *= z;
    return *this;
}
Matrix4& Matrix4::operator=(const Matrix4& other)
{
    for( int i = 0; i < 16; i++)
    {
        m_data[i] = other.m_data[i];
    }
    return *this;
}
Matrix4& Matrix4::operator*=(const Matrix4& other)
{
    *this = *this*other;
    return *this;
}

void Matrix4::Print() const
{
}

void Matrix4::SetAxisVector(int index, const Vec3f& vec)
{
    if(index == 0)
    {
        m_data[0] = vec.GetX(); m_data[1] = vec.GetY(); m_data[2] = vec.GetZ();
    }
    else if (index == 1)
    {
        m_data[4] = vec.GetX(); m_data[5] = vec.GetY(); m_data[6] = vec.GetZ();
    }
    else if(index == 2)
    {
        m_data[8] = vec.GetX(); m_data[9] = vec.GetY(); m_data[10] = vec.GetZ();
    }
    else if (index == 3)
    {
        m_data[12] = vec.GetX(); m_data[13] = vec.GetY(); m_data[14] = vec.GetZ();
    }
}
